from socket import *
import numpy as np
from matplotlib import pyplot as plt
import sys, os, struct
import math
from threading import Thread, Lock
from csi_data import CSI_Data
from read_csi_from_file import read_from_csifile
from keras.models import load_model
from DTW_demo import DTW

PORT = 8899
BUF_SIZE = 155
IP_address = "192.168.1.106"
client = socket(AF_INET, SOCK_STREAM, 0)
client.connect((IP_address, PORT))
totalcnt = 0

def getmod(arr):
    return np.abs(arr)

def getmodl(list):
    return [getmod(arr) for arr in list]

def getphase(arr):
    return np.angle(arr)

def getphasel(list):
    return [getphase(arr) for arr in list]

# 计算相位差
def get_phase_dif(csi1,csi2):
    csi_t = [0] * 30
    for i in range(30):
        if csi1[i] >= 0 and csi2[i] >= 0:
            if csi1[i] >= csi2[i]:
                temp = csi1[i] - csi2[i]
            else:
                temp = csi2[i] - csi1[i]
        elif csi1[i] > 0 and csi2[i] < 0:
            t = csi1[i] - np.pi
            if csi2[i] > t:
                temp = np.pi - csi2[i] + t
            else:
                temp = np.pi * 2 + csi2[i] - csi1[i]
        elif csi1[i] < 0 and csi2[i] > 0:
            t = csi2[i] - np.pi
            if csi1[i] >= t:
                temp = np.pi - csi1[i] + t
            else:
                temp = np.pi * 2 + csi1[i] - csi2[i]
        else:
            if csi1[i] >= csi2[i]:
                temp = csi1[i] - csi2[i]
            else:
                temp = csi2[i] - csi1[i]

        csi_t[i] = temp

    return csi_t


# 相位扩展
def phase_expend(dat):
    csi_pha_exp = [0] * 30
    csi_pha_exp[0] = dat[0]
    for i in range(1,30):
        if (dat[i] - dat[i-1])>=np.pi:
            csi_pha_exp[i] = csi_pha_exp[i-1] + (dat[i] - dat[i-1] - 2*np.pi)
        elif (dat[i] - dat[i-1])<= -np.pi:
            csi_pha_exp[i] = csi_pha_exp[i-1] + (dat[i] - dat[i-1] + 2 * np.pi)
        else:
            csi_pha_exp[i] = csi_pha_exp[i-1] + (dat[i] - dat[i-1])
    return csi_pha_exp

# 相位线性变换
def phase_linear_trans(dat):
    csi_pha_expend = phase_expend(dat)
    csi_pha_trans = [0] * 30
    m = [-28, -26, -24, -22, -20, -18, -16, -14, -12, -10,  -8,  -6,  -4, -2, -1, 1,  3,  5,  7,  9, 11, 13, 15, 17, 19, 21, 23, 25, 27, 28]

    k = (csi_pha_expend[29] - csi_pha_expend[0])/(m[29] - m[0]) #斜率
    b = np.sum(csi_pha_expend)/30 #截距

    for i in range(30):
        csi_pha_trans[i] = csi_pha_expend[i] - k * m[i] - b

    return csi_pha_trans


'''
Hampel滤波
'''
def hampel_filter(dat):
    frame = 5  # hample滤波器的窗口至少为7
    L = 3
    z_ham = []
    for i in range(frame, len(dat[frame:-frame])):
        l = dat[i - frame:i + frame]
        d = np.median(l)
        s = l - d
        m = np.median(s)
        MAD = 1.4826 * m
        if abs(l[0] - d) < L * MAD:
            z_ham.append(l[0])
        else:
            z_ham.append(d)
    # s = np.arange(len(z_ham))
    # b, a = signal.butter(3, 0.03, 'low')
    # sf = signal.filtfilt(b, a, z_ham)
    # plt.plot(s, sf)
    # z_ham = sf
    return z_ham

'''
时域转换到频域,获取频域特征--频域方差
'''
def freq_trans(dat):
    frame = 7
    z_fft = []
    for i in range(frame, len(dat[frame:-frame])):
        l = dat[i - frame:i + frame]
        l_fft = np.fft.fft(l)
        a= np.std(l_fft) #频域方差
        z_fft.append(a)

    return z_fft

'''
获取时域特征
'''
def time_trans(dat):
    frame = 7
    z_var = []
    for i in range(frame, len(dat[frame:-frame])):
        l = dat[i - frame:i + frame]
        l_var = np.std(l)
        z_var.append(l_var)

    return z_var

'''
计算信号短时能量
'''
def calEnergy(dat):
    frame = 7
    z_energy = []
    for i in range(frame, len(dat[frame:-frame])):
        l = dat[i - frame:i + frame]
        a = np.sum(np.square(l))
        z_energy.append(a)

    return z_energy



def read_t():
    alive = True
    while alive:
        try:
            buf = client.recv(BUF_SIZE)
            if buf is None:
                client.close()
                sys.exit(-2)
            i = struct.unpack('>H', buf[:2])
            buf = buf[:i[0]+2]
            # print(len(buf))
            print(i[0])
            if i[0] == len(buf) - 2:  # correct package check - 1
                global totalcnt
                totalcnt += 1
                if buf[2] == 187:  # correct package check - 2
                    dat = CSI_Data(buf[3:])
                    # print(dat)
                    csi_update_pha(dat)
                    # check_motion(dat)
                    # plt.draw()

        except struct.error as e:
            alive=False
            print(e)
            client.close()

model_name = "train_model_NN.h5"

def check_motion(temp_data):  #检测是否有动作发生
    model = load_model(model_name)
    result = model.predict_classes(temp_data,verbose=0)
    check_result.reverse()
    check_result.append(result)
    check_result.reverse()
    check_result.remove(check_result[-1])
    l[1][0].setydat(check_result)
    # plt.draw()


def csi_update_pha(dat):#实时显示相位值
    if dat is not None:
        print(dat.correct)
        csi_t = phase_linear_trans(getphasel(dat.csi[0][0]))#获取0号天线的30个相位
        # print(csi_t[10])
        display_roll.reverse()
        display_roll.append(csi_t[10]) #观察第10号子载波
        display_roll.reverse()
        display_roll.remove(display_roll[-1])

        # print(display_roll)

        # temp = hampel_filter(display_roll)
        # display_roll[0] = temp[0]

        l[0][0].set_ydata(display_roll)
        plt.draw()


num = 200        #显示的x轴数量
display_roll = [0] * num
check_result = [0] * num
plt.figure()
plt.ylim(-5, 1)
l = [None, None, None]
l[0] = plt.plot(range(1, num+1), [0 for _ in range(num)], 'b')
# l[1] = plt.plot(range(1, num+1), [0 for _ in range(num)], 'g')
# l[2] = plt.plot(range(1, num+1), [0 for _ in range(30)], 'g')

read_thread = Thread(target=read_t)

read_thread.start()
plt.show()
read_thread.join()

# read_t()